#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import RPi.GPIO as GPIO
import time
import configparser
import sys
import pdb

LEFT_FRONT_1= 2
LEFT_FRONT_2= 3
LEFT_BEHIND_1 = 4
LEFT_BEHIND_2 = 14

RIGHT_FRONT_1 = 21
RIGHT_FRONT_2 = 26 
RIGHT_BEHIND_1 = 16
RIGHT_BEHIND_2 = 20

class DriveCar():
    
    def __init__(self):
        #config = configparser.ConfigParser()
        #config.read("config.ini")
        # self.LEFT_1 = config.getint("car","left_1")
        # self.LEFT_2 = config.getint("car","left_2")
        # self.RIGHT_1 = config.getint("car","right_1")
        # self.RIGHT_2 = config.getint("car","right_2")
        # self.LEFT_1 = 2
        # self.LEFT_2 = 3
        # self.RIGHT_1 = 4
        # self.RIGHT_2 = 14

        GPIO.setmode(GPIO.BCM)
        GPIO.setwarnings(False)
        GPIO.setup(LEFT_FRONT_1,GPIO.OUT)
        GPIO.setup(LEFT_FRONT_2,GPIO.OUT)
        GPIO.setup(LEFT_BEHIND_1,GPIO.OUT)
        GPIO.setup(LEFT_BEHIND_2,GPIO.OUT)
        GPIO.setup(RIGHT_FRONT_1,GPIO.OUT)
        GPIO.setup(RIGHT_FRONT_2,GPIO.OUT)
        GPIO.setup(RIGHT_BEHIND_1,GPIO.OUT)
        GPIO.setup(RIGHT_BEHIND_2,GPIO.OUT)


    def reset(self):
        GPIO.output(LEFT_FRONT_1,GPIO.LOW)
        GPIO.output(LEFT_FRONT_2,GPIO.LOW)
        GPIO.output(LEFT_BEHIND_1,GPIO.LOW)
        GPIO.output(LEFT_BEHIND_2,GPIO.LOW)
        GPIO.output(RIGHT_FRONT_1,GPIO.LOW)
        GPIO.output(RIGHT_FRONT_2,GPIO.LOW)
        GPIO.output(RIGHT_BEHIND_1,GPIO.LOW)
        GPIO.output(RIGHT_BEHIND_2,GPIO.LOW)

    def __forward(self):
        self.reset()
        GPIO.output(LEFT_FRONT_1,GPIO.HIGH)
        GPIO.output(LEFT_FRONT_2,GPIO.LOW)
        GPIO.output(LEFT_BEHIND_1,GPIO.HIGH)
        GPIO.output(LEFT_BEHIND_2,GPIO.LOW)
        GPIO.output(RIGHT_FRONT_1,GPIO.HIGH)
        GPIO.output(RIGHT_FRONT_2,GPIO.LOW)
        GPIO.output(RIGHT_BEHIND_1,GPIO.HIGH)
        GPIO.output(RIGHT_BEHIND_2,GPIO.LOW)
        

    def __backword(self):
        self.reset()
        GPIO.output(LEFT_FRONT_1,GPIO.LOW)
        GPIO.output(LEFT_FRONT_2,GPIO.HIGH)
        GPIO.output(LEFT_BEHIND_1,GPIO.LOW)
        GPIO.output(LEFT_BEHIND_2,GPIO.HIGH)
        GPIO.output(RIGHT_FRONT_1,GPIO.LOW)
        GPIO.output(RIGHT_FRONT_2,GPIO.HIGH)
        GPIO.output(RIGHT_BEHIND_1,GPIO.LOW)
        GPIO.output(RIGHT_BEHIND_2,GPIO.HIGH)

    def __turnleft(self):
        self.reset()
        GPIO.output(LEFT_FRONT_1,GPIO.LOW)
        GPIO.output(LEFT_FRONT_2,GPIO.HIGH)
        GPIO.output(LEFT_BEHIND_1,GPIO.LOW)
        GPIO.output(LEFT_BEHIND_2,GPIO.HIGH)
        GPIO.output(RIGHT_FRONT_1,GPIO.HIGH)
        GPIO.output(RIGHT_FRONT_2,GPIO.LOW)
        GPIO.output(RIGHT_BEHIND_1,GPIO.HIGH)
        GPIO.output(RIGHT_BEHIND_2,GPIO.LOW)

    def __turnright(self):
        self.reset()
        GPIO.output(LEFT_FRONT_1,GPIO.HIGH)
        GPIO.output(LEFT_FRONT_2,GPIO.LOW)
        GPIO.output(LEFT_BEHIND_1,GPIO.HIGH)
        GPIO.output(LEFT_BEHIND_2,GPIO.LOW)
        GPIO.output(RIGHT_FRONT_1,GPIO.LOW)
        GPIO.output(RIGHT_FRONT_2,GPIO.HIGH)
        GPIO.output(RIGHT_BEHIND_1,GPIO.LOW)
        GPIO.output(RIGHT_BEHIND_2,GPIO.HIGH) 
    
    def __backleft(self):
        self.reset()
        GPIO.output(LEFT_FRONT_1,GPIO.LOW)
        GPIO.output(LEFT_FRONT_2,GPIO.HIGH)
        GPIO.output(LEFT_BEHIND_1,GPIO.LOW)
        GPIO.output(LEFT_BEHIND_2,GPIO.HIGH)
        GPIO.output(RIGHT_FRONT_1,GPIO.LOW)
        GPIO.output(RIGHT_FRONT_2,GPIO.HIGH)
        GPIO.output(RIGHT_BEHIND_1,GPIO.LOW)
        GPIO.output(RIGHT_BEHIND_2,GPIO.HIGH)

    def __backright(self):    
        self.reset()
        GPIO.output(LEFT_FRONT_1,GPIO.LOW)
        GPIO.output(LEFT_FRONT_2,GPIO.HIGH)
        GPIO.output(LEFT_BEHIND_1,GPIO.LOW)
        GPIO.output(LEFT_BEHIND_2,GPIO.HIGH)
        GPIO.output(RIGHT_FRONT_1,GPIO.LOW)
        GPIO.output(RIGHT_FRONT_2,GPIO.HIGH)
        GPIO.output(RIGHT_BEHIND_1,GPIO.LOW)
        GPIO.output(RIGHT_BEHIND_2,GPIO.HIGH)

    def __stop(self):
        self.reset()

    def carMove(self, direction):
        '''
        根据输入的参数，移动小车的位置
        '''  
        if direction == 'f':
            self.__forward()
        elif direction == 'b':
            self.__backword()
        elif direction == 'r':
            self.__turnright()
        elif direction == 'l':
            self.__turnleft()
        elif direction == 'bl':
            self.__backleft()
        elif direction == 'br':
            self.__backright()
        elif direction == 's':
            self.__stop()
        else:
            '''
            请输入你要控制小车的方向：前进: F 后退: B 左转: L 右转: R 左后转向: BL 右后转向: BR
            '''
            print("请输入你要控制小车的方向：前进: f 后退: b 左转: l 右转: r 左后转向: bl 右后转向: br 停止：s")

if __name__ == "__main__":
    raspCar =DriveCar()
    while(True):
        direction = input("Please input direction: ")
        print(direction);
        raspCar.carMove(direction)
        time.sleep(2)

  